#!/usr/bin/env python
import rospy
from std_msgs.msg import String
from tf_conversions import transformations
from math import pi
import tf

class Robot:
    def __init__(self):
	rospy.init_node('get_pose',anonymous=True)

        self.tf_listener = tf.TransformListener()
        self.getpose_read = rospy.Subscriber("read",String,self.callback)
        self.getpose_pub = rospy.Publisher('write',String,queue_size=1)
        try:
            self.tf_listener.waitForTransform('/map', '/base_link', rospy.Time(), rospy.Duration(1.0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            return
    
    def callback(self,data):
	rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
        if(data.data=="g"):     ##
		try:
            		(trans, rot) = self.tf_listener.lookupTransform('/map', '/base_link', rospy.Time(0))
        	except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            		rospy.loginfo("tf Error")
            		return None
        	euler = transformations.euler_from_quaternion(rot)
        	#print euler[2] / pi * 180

        	x = trans[0]
        	y = trans[1]
        	th = euler[2] / pi * 180  
                pose = "(" + str(x) + "," + str(y) + "," + str(th) + ")"
                rospy.loginfo(pose)
                self.getpose_pub.publish(pose)     

if __name__ == "__main__":
    try:
	    Robot()
            rospy.spin()
	    #robot = Robot()
	    #r = rospy.Rate(100)
	    #r.sleep()
	    #while not rospy.is_shutdown():
		#print robot.get_pos()
		#r.sleep()
    except rospy.ROSInterruptException:
            rospy.loginfo("get_pose finished.")
